/*
 * soc.c
 * 简单 SOC 算法实现：库仑计数（累计 mAh）+ 静止时 OCV 校正
 */

#include "soc.h"
#include <string.h>

/* 默认参数 */
static float g_capacity_mAh = 1000.0f; /* 默认 1000mAh */
static float g_soc_pct = 50.0f;

/* 累计库仑（mAh）——已放电量，范围 0..capacity */
static float g_used_mAh = 0.0f;

/* OCV 查表（示例）：电压 mV -> SOC %，升序 */
static const uint16_t default_ocv_volt[] = {3000, 3300, 3600, 3700, 3800, 3900, 4000, 4100, 4200};
static const uint8_t  default_ocv_soc[]  = {0,    5,    20,   40,   60,   75,   85,   95,   100};
static const size_t   default_ocv_len = sizeof(default_ocv_volt)/sizeof(default_ocv_volt[0]);

static const uint16_t *g_ocv_volt = default_ocv_volt;
static const uint8_t  *g_ocv_soc  = default_ocv_soc;
static size_t g_ocv_len = default_ocv_len;

/* 状态：上次更新时间戳可由调用方提供；这里仅依赖 dt_s 参数 */

void SOC_Init(float capacity_mAh, float initial_soc_percent)
{
    if (capacity_mAh > 0.0f) {
        g_capacity_mAh = capacity_mAh;
    }
    if (initial_soc_percent < 0.0f) initial_soc_percent = 0.0f;
    if (initial_soc_percent > 100.0f) initial_soc_percent = 100.0f;
    g_soc_pct = initial_soc_percent;
    g_used_mAh = (1.0f - g_soc_pct/100.0f) * g_capacity_mAh;
}

void SOC_SetCapacity(float capacity_mAh)
{
    if (capacity_mAh <= 0.0f) return;
    /* 根据新的容量按比例调整 used_mAh 保持相同 SOC */
    float soc = g_soc_pct/100.0f;
    g_capacity_mAh = capacity_mAh;
    g_used_mAh = (1.0f - soc) * g_capacity_mAh;
}

int SOC_SetOCVTable(const uint16_t *volt_table_mV, const uint8_t *soc_table_pct, size_t len)
{
    if (!volt_table_mV || !soc_table_pct || len < 2) return -1;
    g_ocv_volt = volt_table_mV;
    g_ocv_soc  = soc_table_pct;
    g_ocv_len  = len;
    return 0;
}

/* 线性插值 OCV->SOC */
static float ocv_lookup_soc(float volt_mV)
{
    if (g_ocv_len == 0) return g_soc_pct;
    if (volt_mV <= g_ocv_volt[0]) return (float)g_ocv_soc[0];
    if (volt_mV >= g_ocv_volt[g_ocv_len-1]) return (float)g_ocv_soc[g_ocv_len-1];
    for (size_t i = 1; i < g_ocv_len; ++i) {
        if (volt_mV <= g_ocv_volt[i]) {
            float x0 = g_ocv_volt[i-1];
            float x1 = g_ocv_volt[i];
            float y0 = g_ocv_soc[i-1];
            float y1 = g_ocv_soc[i];
            float t = (volt_mV - x0) / (x1 - x0);
            return y0 + t * (y1 - y0);
        }
    }
    return g_ocv_soc[g_ocv_len-1];
}

/* 更新 SOC
 * current_mA > 0 表示放电（电池输出），则 used_mAh 增加
 */
void SOC_Update(float voltage_mV, float current_mA, float dt_s)
{
    if (dt_s <= 0.0f) return;

    /* 库仑计数：current (mA) * time (h) = mAh */
    float delta_h = dt_s / 3600.0f;
    g_used_mAh += current_mA * delta_h;

    /* 限制范围 */
    if (g_used_mAh < 0.0f) g_used_mAh = 0.0f;
    if (g_used_mAh > g_capacity_mAh) g_used_mAh = g_capacity_mAh;

    /* 暂时的 SOC 估计来自库仑计数 */
    float soc_from_coulomb = (1.0f - g_used_mAh / g_capacity_mAh) * 100.0f;

    /* 当电流接近 0（静止）时，用 OCV 校正 SOC */
    const float idle_current_threshold_mA = 50.0f; /* 小于此值认为静止（示例值） */
    if ( (current_mA > -idle_current_threshold_mA) && (current_mA < idle_current_threshold_mA) ) {
        float soc_ocv = ocv_lookup_soc(voltage_mV);
        /* 简单融合：若 OCV 与 库仑估计差别较大，以加权平均靠近 OCV */
        float diff = soc_ocv - soc_from_coulomb;
        /* 如果差别大于阈值，则逐步校正以避免跳变 */
        if (fabsf(diff) > 1.0f) {
            /* 每次校正步长，比例因子 */
            float adjust = diff * 0.2f; /* 20% 修正 */
            g_soc_pct = soc_from_coulomb + adjust;
        } else {
            g_soc_pct = soc_ocv; /* 接近则直接采用 OCV */
        }
    } else {
        /* 在有明显电流时，跟随库仑计数估计 */
        g_soc_pct = soc_from_coulomb;
    }

    /* 限制 0..100 */
    if (g_soc_pct < 0.0f) g_soc_pct = 0.0f;
    if (g_soc_pct > 100.0f) g_soc_pct = 100.0f;
}

float SOC_GetPercent(void)
{
    return g_soc_pct;
}
